#!/usr/bin/python                             
#-*- coding:utf-8 -*-  

import RPi.GPIO as GPIO
import time
import rospy
from robot_pkg.msg import Person1


# w1,w2,w3,w4,w5,w6 = 0,1,0,0,0,0,0   细分400，电流3.5A，电压24V
IN1 = 20  # 接PUL-
IN2 = 21  # 接PUL+
IN3 = 12  # 接DIR-
IN4 = 16  # 接DIR+
Relays3_GPIO = 6 #继电器3GPIO 高电平使能

def setStep(w1, w2, w3, w4):
    GPIO.output(IN1, w1)
    GPIO.output(IN2, w2)
    GPIO.output(IN3, w3)
    GPIO.output(IN4, w4)

def stop():
    setStep(0, 0, 0, 0)

# 正转
# 控制电机旋转的快慢和圈数 delay越小转得越快，1600为一圈
def forward(delay, steps):
    for i in range(0, steps):
        setStep(1, 0, 1, 0)
        time.sleep(delay)
        setStep(0, 1, 1, 0)
        time.sleep(delay)
        setStep(0, 1, 0, 1)
        time.sleep(delay)
        setStep(1, 0, 0, 1)
        time.sleep(delay)

# 反转
# 控制电机旋转的快慢和圈数 delay越小转得越快，1600为一圈
def backward(delay, steps):
    for i in range(0, steps):
        setStep(1, 0, 0, 1)
        time.sleep(delay)
        setStep(0, 1, 0, 1)
        time.sleep(delay)
        setStep(0, 1, 1, 0)
        time.sleep(delay)
        setStep(1, 0, 1, 0)
        time.sleep(delay)

# 初始化树莓派引脚，设置树莓派的引脚为输出状态
def setup():
    GPIO.setwarnings(False)
    GPIO.setmode(GPIO.BCM)
    GPIO.setup(IN1, GPIO.OUT)
    GPIO.setup(IN2, GPIO.OUT)
    GPIO.setup(IN3, GPIO.OUT)
    GPIO.setup(IN4, GPIO.OUT)
    #继电器3
    GPIO.setup(Relays3_GPIO, GPIO.OUT)
    GPIO.output(Relays3_GPIO, 0)
    

# 清除树莓派引脚状态赋值            
def destroy():
    GPIO.cleanup()  # 释放数据
    
#控制继电器
def ConrtolRelays(Relays_GPIO,data):
	if data == 1:#打开
		GPIO.output(Relays_GPIO, 1)
	if data == 2:#关闭
		GPIO.output(Relays_GPIO, 0)
# 获取服务端数据
Gears = 5 #步进电机档位
def get_data(msg):
    global Gears
    if ((Gears > 5) | (Gears < 0 )):
        print("步进电机档位错误:",Gears)
        return 0
    #处理继电器3
    ConrtolRelays(Relays3_GPIO,msg.Relays3)

    if Gears - msg.StepperDirection < 0 :# 正转
        Gears = msg.StepperDirection
        print("步进电机正转")
        forward(0.0001, 1600)#正转
        time.sleep(3)
        stop()
    if Gears - msg.StepperDirection > 0 :# 反转
        Gears = msg.StepperDirection
        print("步进电机反转")
        backward(0.0001, 1600)#反转
        time.sleep(3)
        stop()  # stop
    #出现错误
    if msg.error == 1:
        print("步进电机关闭")
        if Gears != 5:
            for bright in range(0, Gears):
                print("步进电机正转")
                forward(0.0001, 1600)#正转
                time.sleep(1)
                stop()
        GPIO.output(Relays3_GPIO, 1)
	

def main():
	rospy.init_node('StepperMotors', anonymous=True)    
	rospy.Subscriber("/client_publisher", Person1, get_data,queue_size=100)
	rospy.spin()

if __name__ == '__main__':  # Program start from here
   setup()
   try:
        main()
   except KeyboardInterrupt:  # When 'Ctrl+C' is pressed, the child function destroy() will be  executed.
        destroy()
        
        
        
        
        
